////////////////////////////////////////////////////////////
// Headers
////////////////////////////////////////////////////////////
#include "BasePathfindingAlgorithm.hpp"

#include "../Grid.hpp"

#ifdef _DEBUG
#include <sstream>
#include <iomanip>
#include <SFML/Graphics.hpp>
#include <SmegCore/Math/Math.hpp>
#endif

#include <SmegTools/Debug/DebugNew.hpp>

namespace smeg {
    namespace ai {

smeg::tools::Pool< Node > BasePathfindingAlgorithm::s_PoolNode  = smeg::tools::Pool< Node >();
sf::Vector2u BasePathfindingAlgorithm::s_Sides[8]               = {
    // Straights
    sf::Vector2u( 0, 1 ),
    sf::Vector2u( 1, 0 ),
    sf::Vector2u( 0, -1 ),
    sf::Vector2u( -1, 0 ),
    // Diagonals
    sf::Vector2u( 1, 1 ),
    sf::Vector2u( 1, -1 ),
    sf::Vector2u( -1, -1 ),
    sf::Vector2u( -1, 1 ),
};
        
////////////////////////////////////////////////////////////
/// Default Constructor
////////////////////////////////////////////////////////////
BasePathfindingAlgorithm::BasePathfindingAlgorithm() :
    m_pGrid( NULL )
#ifdef _DEBUG
    , DEBUG_Font( NULL )
    , DEBUG_Text( NULL )
#endif
{
}

////////////////////////////////////////////////////////////
/// Constructor
////////////////////////////////////////////////////////////
BasePathfindingAlgorithm::BasePathfindingAlgorithm( Grid* _pGrid ) :
    m_pGrid( _pGrid )
#ifdef _DEBUG
    , DEBUG_Font( NULL )
    , DEBUG_Text( NULL )
#endif
{
}


////////////////////////////////////////////////////////////
/// Destructor
////////////////////////////////////////////////////////////
BasePathfindingAlgorithm::~BasePathfindingAlgorithm()
{
#ifdef _DEBUG
    delete DEBUG_Font;
    delete DEBUG_Text;
#endif
}

////////////////////////////////////////////////////////////
/// Clear last found path infos
////////////////////////////////////////////////////////////
void BasePathfindingAlgorithm::ClearPath() {
    m_LastFoundPath.clear();
    
    m_OpenSet.clear();
    m_ClosedMap.clear();
    m_DoneNodes.clear();
}

////////////////////////////////////////////////////////////
/// Reconstruct a path from the given node to its farthest parent
////////////////////////////////////////////////////////////
void BasePathfindingAlgorithm::ReconstructPath( Node* _pLastNode ) {
    Node* pCurNode = _pLastNode;
    sf::Vector2f tmpPoint;

    m_LastFoundPath.clear();
    while ( pCurNode ) {
        m_pGrid->ConvertToWorldSpace( pCurNode->X, pCurNode->Y, tmpPoint );
        m_LastFoundPath.push_back( tmpPoint );

        pCurNode = pCurNode->parent;
    }

    std::reverse( m_LastFoundPath.begin(), m_LastFoundPath.end() );
}

////////////////////////////////////////////////////////////
/// Heuristic function to compute the F value of a Node
////////////////////////////////////////////////////////////
float BasePathfindingAlgorithm::Heuristic( const sf::Vector2i& _Point, const sf::Vector2i& _End ) {
    // TODO For now, using Manhattan heuristic
    return abs( (float)_End.x - (float)_Point.x ) + abs( (float)_End.y - (float)_Point.y );
}

#ifdef _DEBUG
void BasePathfindingAlgorithm::DebugRenderFindPath( sf::RenderWindow* _pRenderWindow ) {
    const float fCellSize = (float)m_pGrid->GetCellSize(),
        fHalfCellSize = fCellSize * 0.5f;
    sf::RectangleShape drawRect( sf::Vector2f( fCellSize, fCellSize ) );
    drawRect.setFillColor( sf::Color::Transparent );
    drawRect.setOutlineThickness( -2.0f );

    // Draw from child to parent
    sf::Vertex lines[2] = {
        sf::Vertex( sf::Vector2f(), sf::Color::Magenta ),
        sf::Vertex( sf::Vector2f(), sf::Color::Magenta )
    };
    
    if ( !DEBUG_Font ) {
        DEBUG_Font = new sf::Font();
        DEBUG_Font->loadFromFile( "data/fonts/COUR.ttf" );
    }
    if ( !DEBUG_Text ) {
        DEBUG_Text = new sf::Text( "", *DEBUG_Font, 9 );
        DEBUG_Text->setColor( sf::Color::White );
    }

    Node* pCurrentNode;
    sf::Vector2f CurrentNodePos,
        HalfNodePos( 0.5f * fCellSize, 0.5f * fCellSize ),
        ParentToNode;
    std::stringstream sstr;

    // Draw the open list
    drawRect.setOutlineColor( sf::Color::Cyan );
    for ( td_PathPriorityQueue::iterator it = m_OpenSet.begin(); 
        it != m_OpenSet.end();
        ++it ) {
        pCurrentNode = *it;
        CurrentNodePos.x = pCurrentNode->X * fCellSize;
        CurrentNodePos.y = pCurrentNode->Y * fCellSize;

        drawRect.setPosition( CurrentNodePos );
        _pRenderWindow->draw( drawRect );
        
        if ( fHalfCellSize > (float)DEBUG_Text->getCharacterSize() ) {
            // Draw F
            sstr.str("");
            sstr << std::setprecision( 2 ) << pCurrentNode->F;
            DEBUG_Text->setPosition( CurrentNodePos.x + 0.1f * fCellSize, CurrentNodePos.y );
            DEBUG_Text->setString( sstr.str() );
            _pRenderWindow->draw( *DEBUG_Text );
        
            // Draw G
            sstr.str("");
            sstr << std::setprecision( 2 ) << pCurrentNode->G;
            DEBUG_Text->setPosition( CurrentNodePos.x + 0.1f * fCellSize, CurrentNodePos.y + 0.7f * fCellSize );
            DEBUG_Text->setString( sstr.str() );
            _pRenderWindow->draw( *DEBUG_Text );
        
            // Draw H
            sstr.str("");
            sstr << std::setprecision( 2 ) << (pCurrentNode->F - pCurrentNode->G);
            DEBUG_Text->setPosition( CurrentNodePos.x + 0.6f * fCellSize, CurrentNodePos.y + 0.7f * fCellSize );
            DEBUG_Text->setString( sstr.str() );
            _pRenderWindow->draw( *DEBUG_Text );

            // Draw from node to parent
            if ( pCurrentNode->parent ) {
                lines[0].position = CurrentNodePos + HalfNodePos;

                ParentToNode.x = (float)pCurrentNode->parent->X - pCurrentNode->X;
                ParentToNode.y = (float)pCurrentNode->parent->Y - pCurrentNode->Y;
                math::Normalize( ParentToNode, fHalfCellSize );

                lines[1].position = lines[0].position + ParentToNode;
                _pRenderWindow->draw( lines, 2, sf::Lines );
            }
        }
    }

    // Draw the closed list
    drawRect.setOutlineColor( sf::Color::Green );
    for ( td_PathMap::iterator it = m_ClosedMap.begin(); 
        it != m_ClosedMap.end();
        ++it ) {
        pCurrentNode = it->second;
        CurrentNodePos.x = pCurrentNode->X * fCellSize;
        CurrentNodePos.y = pCurrentNode->Y * fCellSize;

        drawRect.setPosition( CurrentNodePos );
        _pRenderWindow->draw( drawRect );
        
        if ( fHalfCellSize > (float)DEBUG_Text->getCharacterSize() ) {
            // Draw F
            sstr.str("");
            sstr << std::setprecision( 2 ) << pCurrentNode->F;
            DEBUG_Text->setPosition( CurrentNodePos.x + 0.1f * fCellSize, CurrentNodePos.y );
            DEBUG_Text->setString( sstr.str() );
            _pRenderWindow->draw( *DEBUG_Text );
        
            // Draw G
            sstr.str("");
            sstr << std::setprecision( 2 ) << pCurrentNode->G;
            DEBUG_Text->setPosition( CurrentNodePos.x + 0.1f * fCellSize, CurrentNodePos.y + 0.7f * fCellSize );
            DEBUG_Text->setString( sstr.str() );
            _pRenderWindow->draw( *DEBUG_Text );
        
            // Draw H
            sstr.str("");
            sstr << std::setprecision( 2 ) << (pCurrentNode->F - pCurrentNode->G);
            DEBUG_Text->setPosition( CurrentNodePos.x + 0.6f * fCellSize, CurrentNodePos.y + 0.7f * fCellSize );
            DEBUG_Text->setString( sstr.str() );
            _pRenderWindow->draw( *DEBUG_Text );

            // Draw from node to parent
            if ( pCurrentNode->parent ) {
                lines[0].position = CurrentNodePos + HalfNodePos;

                ParentToNode.x = (float)pCurrentNode->parent->X - pCurrentNode->X;
                ParentToNode.y = (float)pCurrentNode->parent->Y - pCurrentNode->Y;
                math::Normalize( ParentToNode, fHalfCellSize );

                lines[1].position = lines[0].position + ParentToNode;
                _pRenderWindow->draw( lines, 2, sf::Lines );
            }
        }
    }
    
    drawRect.setOutlineColor( sf::Color::Yellow );
    lines[0].color = sf::Color::Yellow;
    lines[1].color = sf::Color::Yellow;
    for ( unsigned i = 1; i < m_LastFoundPath.size(); ++i ) {
        drawRect.setPosition( m_LastFoundPath[i].x, m_LastFoundPath[i].y );
        _pRenderWindow->draw( drawRect );

        // Draw path
        lines[0].position = m_LastFoundPath[i - 1] + HalfNodePos;
        lines[1].position = m_LastFoundPath[i] + HalfNodePos;
        _pRenderWindow->draw( lines, 2, sf::Lines );
    }
}
#endif

    } // namespace ai
} // namespace smeg